#define     ZOOM_T_GLOBAL
/************includes*******************/
#include "ZoomTrace.h"
#include "ipz_zbc_includes.h"
SW_INT IfADD = 0;
SW_INT BakIfADD = 0;
SW_INT StageCase = 0;
SW_INT ContinueDownCnt = 0;
SW_INT ContinueUpCnt = 0;
SW_INT AdjStep = 0;
SW_INT AdjStepCase = 0;
SW_INT DelayTime = 4;
SW_INT16 LensPredictCurve[150];
void ZoomTParaInit(void)
{
    printf("=======================================\n");
    printf("=======================================build time[%s]\n",__DATE__);
    printf("=======================================\n");
}
void ZoomTHookEnter(void)
{
    IfADD = 1;
    StageCase = 0;
    ContinueDownCnt = 0;
    ContinueUpCnt = 0;
    DelayTime = 4;
    BakIfADD = 0;
    AdjStepCase = 0;
}
void UpdateAdjDir(void)
{
    if(1 == IfADD)
    {
        IfADD = -1;
    }
    else if(-1 == IfADD)
    {
        IfADD = 1;
    }
    else if(0 == IfADD)
    {
        IfADD = 1;
    }
}
void DelayTimeDecrease(void)
{
    if(DelayTime > 0)
    {
        DelayTime--;
    }
}
void GetFocusIntPosRate(SW_INT16 s16CurZoom,SW_INT16 s16NextZoom)
{
    SW_INT idx, i,j,k, temp, curr, pos;
    SW_INT16 *r[7] = {Lens1M5, Lens3M, Lens6M, Lens10M,Lens20M,Lens50M,LensInf}; // 1.5M 3M 6M INF
    SW_FLOAT *ra[5] = {NULL, NULL, NULL, NULL,NULL}; // 1.5M 3M 6M INF
    SW_INT16 data[7], diff[7], minus, plus;
    SW_FLOAT f32CurRate,f32NextRate;
    ZoomRateDef pstru;
    pstru = ZoomStru;
    curr = IPZ_ZBC_LENS_GetFocusCurPos();
    pos = IPZ_ZBC_LENS_GetZoomCurPos();
    r[0] = Lens1M5;
    r[1] = Lens3M;
    r[2] = Lens6M;
    r[3] = Lens10M;
    r[4] = Lens20M;
    r[5] = Lens50M;
    r[6] = LensInf;
    for(i=0; i<7; i++)
    {
        data[i] = IPZ_ZBC_CURVE_GetCurveFocus(pos, (SW_INT16*)(r[i]));
        diff[i] = curr - data[i];
    }
    plus = 9999;
    minus = -9999;
    if( curr >= data[0])
    {
        pstru.Extra = SW_B_TRUE;
        pstru.PredictCurve1 = (SW_INT16*)(r[0]);
        pstru.PredictCurve2 = NULL;
        pstru.Rate = 0.0;
        ZoomStru = pstru;
        return;
    }
    else if( curr <= data[6])
    {
        pstru.Extra = SW_B_TRUE;
        pstru.PredictCurve1 = (SW_INT16*)(r[6]);
        pstru.PredictCurve2 = NULL;
        pstru.Rate = 0.0;
        ZoomStru = pstru;
        return;
    }
    else
    {
        j = 0;
        idx = 6;
        for(i=0; i<7; i++)
        {
            if( diff[i] > 0)
            {
                if( diff[i] <= plus)
                {
                    plus = diff[i];
                    idx = i;
                }
            }
            else if( diff[i] <0)
            {
                if( diff[i] > minus)
                {
                    minus = diff[i];
                    j = i;
                }
            }
            else
            {
                pstru.Extra = SW_B_TRUE;
                pstru.PredictCurve1 = (SW_INT16*)(r[i]);
                pstru.PredictCurve2 = NULL;
                pstru.Rate = 0.0;
                ZoomStru = pstru;
                return;
            }
        }
        temp = SW_ABS(minus) + SW_ABS(plus);
        pstru.Extra = SW_B_FALSE;
        pstru.PredictCurve1 = (SW_INT16*)(r[j]);
        pstru.PredictCurve2 = (SW_INT16*)(r[idx]);
        pstru.Rate = (SW_FLOAT)(SW_ABS(plus)) / temp;
        ZoomStru = pstru;
    }
    return;
}
#ifndef MIN
#define MIN(x, y)                            (((x) <= (y)) ? (x) : (y))
#endif
#ifndef MAX
#define MAX(x, y)                            (((x) >= (y)) ? (x) : (y))
#endif
#define MID(x, v, y)                         (MIN(MAX((x), (v)), (y)))
#define DIV(x, y)                            (((y)==0) ? 0 : ((x) * 1.0) / (y))
#define MAP(sx, sv, sy, dx, dy)              (MID((dx), (DIV((((sv) - (sx)) * ((dy) - (dx))), ((sy) - (sx))) + (dx)), (dy)))
#define MAP2(sx, sv, sy, dx, dy)             (((dx) <= (dy)) ? (MAP((sx), (sv), (sy), (dx), (dy))) : ((dx) + (dy) - MAP((sx), (sv), (sy), (dy), (dx))))
int32_t zoom1_curve[100] = {0,600,1136,1592,2016,2400,2760,3240,3672,4016,4224,4488,4920,5336,5752,5864,6048,6776,7160,7296,7560,7824,8080,8304,8520,8712,8896,8928,9032,9184,9328,9464,9600,9736,9864,9976,10096,10200,10296,10384,10472,10568,10656,10744,10832,10912,10976,11048,11120,11152,11208,11264,11320,11368,11416,11456,11488,11520,11560,11576,11608,11640,11672,11696,11712,11736,11752,11768,11784,11800,11816,11840,11856,11872,11880,11888,11896,11904,11920,11928,11936,11944,11952,11960,11968,11976,11984,11992,12000,};
float focal_length_curve[100] = {7.10,7.84,8.58,9.27,9.98,10.67,11.38,12.40,13.42,14.32,14.91,15.68,17.05,18.53,20.21,20.70,21.58,25.57,27.67,28.46,30.46,32.47,34.42,36.48,38.63,40.53,42.41,42.80,44.09,45.97,47.75,49.49,51.53,53.57,55.50,57.35,59.50,61.37,63.09,64.98,66.90,69.00,70.99,73.27,75.55,77.74,79.78,82.07,84.37,85.48,87.72,89.96,92.41,94.73,97.06,99.17,101.06,102.95,105.32,106.26,108.71,111.15,113.92,116.23,117.77,120.15,121.96,123.78,125.59,127.41,129.84,133.50,136.46,139.58,141.15,142.77,144.46,146.14,149.68,151.71,153.73,155.76,157.94,160.12,162.31,164.64,166.97,169.31,172.05,};
int32_t binary( int32_t *a, int32_t key, int32_t n )
{
    int32_t left = 0, right = n - 1, mid = 0;
    while( left <= right )
    {
        mid = ( left + right ) / 2;
        if ( ( a[mid] <= key ) && ( key <= a[mid+1] ) )
        {
            return mid;
        }
        if ( a[mid] < key )
        {
            left = mid + 1;
        }
        else if( a[mid] > key )
        {
            right = mid - 1;
        }
    }
    return -1;
}
void RunMotorAndGetZoomTraceInfo(SW_INT NO)
{
    SW_INT i, suml, sumh;
    SW_INT field;
    SW_INT zoom = IPZ_ZBC_LENS_GetZoomCurPos();
    SW_INT zoom_idx;
    SW_INT idx;
    SW_INT lenstType;
    SW_INT TraceHighFreData_backup = 0;
    int32_t curve_index;
    float focal_length;
    lenstType = IPZ_ZBC_DEV_GetLensType();
    IPZ_ZBC_FILTER_SetVDFlag(1,AFD_READ_AFD);
    if(lenstType == IPZ_ZBC_DEV_LENS_UNION_T5719)
    {
        SW_TimeLockMutex(hMotorMutex,-1);
        IPZ_ZBC_MOTOR_MotorRun(DRIVER_ZOOM_FOCUS_ZOOMC_FLAG);
        SW_UnlockMutex(hMotorMutex);
    }
    else
    {
        SW_TimeLockMutex(hMotorMutex,-1);
        IPZ_ZBC_MOTOR_MotorRun(DRIVER_ZOOM_FOCUS_FLAG);
        SW_UnlockMutex(hMotorMutex);
    }
    SW_TimeWaitSem(pAFDSem,5000);
    GetAFD_Info();
    suml =  NowAFD.SumL;
    sumh =  NowAFD.SumH;
    for(i = sizeof(ZoomTParaList.TraceData)/sizeof(ZoomTParaList.TraceData[0]) - 1; i>=1; i--)
    {
        ZoomTParaList.TraceData[i] = ZoomTParaList.TraceData[i-1];
        ZoomTParaList.TraceHighFreData[i] = ZoomTParaList.TraceHighFreData[i-1];
    }
    ZoomTParaList.TraceData[0] = suml;
    ZoomTParaList.TraceHighFreData[0] = sumh;
    curve_index = binary(zoom1_curve, zoom, 89);
    focal_length = MAP2(zoom1_curve[curve_index], zoom, zoom1_curve[curve_index+1], focal_length_curve[curve_index], focal_length_curve[curve_index+1]);
    AdjStep = focal_length * 0.6;
    AdjStep = AdjStep < 1 ? 1 : AdjStep;
    AdjStep = AdjStep > 20 ? 20 : AdjStep;
    if(zoom < 2016)
    {
        AdjStep = 0;
    }
    printf("zoom = %d, focal_length = %f, AdjStep = %d\n", zoom, focal_length, AdjStep);
    switch(StageCase)
    {
    case 0:
		DelayTimeDecrease();
        if(0 == DelayTime)
        {
            StageCase = 1;
        }
        break;
    case 1:
        if(ZoomTParaList.TraceData[0] >= ZoomTParaList.TraceData[1])
        {
            BakIfADD = IfADD;
        }
        else
        {
            UpdateAdjDir();
            StageCase = 0;
            DelayTime = 4;
        }
        break;
    default:
        ZoomTHookEnter();
        break;
    }
}
SW_BOOL DevDriverZoomAJ(SW_INT step, SW_INT N)
{
    SW_INT zoom_p, zoom, next_focus_pos, stepDiff;
    SW_INT focus, focus_next_min, focus_next_max;
    SW_INT16 *upper_curve, *lower_curve, *limit_curve;
    ZoomRateDef  pz;
    SW_INT16 *f1, *f2;
    SW_FLOAT ratio;
    static SW_FLOAT ratio_bak;
    SW_INT lens_type;
    lens_type = IPZ_ZBC_DEV_GetLensType();
    pz = (ZoomRateDef)ZoomStru;
    f1 = pz.PredictCurve1;
    f2 = pz.PredictCurve2;
	zoom = IPZ_ZBC_LENS_GetZoomCurPos();
	focus = IPZ_ZBC_LENS_GetFocusCurPos();
	if(IPZ_ZBC_LENS_GetZoomDir())
	{
		zoom_p = zoom - IPZ_ZBC_LENS_GetZoomStepNum();
	}
	else
	{
		zoom_p = zoom + IPZ_ZBC_LENS_GetZoomStepNum();
	}
	limit_curve = CurveLimitIdx;
	if(NULL != limit_curve)
	{
		upper_curve = CurveLimitIdx;
		lower_curve = LensInf;
	}
	else
	{
		upper_curve = Lens1M5;
		lower_curve = LensInf;
	}
	focus_next_max = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, upper_curve);
	focus_next_min = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, lower_curve);
	GetFocusIntPosRate(zoom,zoom_p);
	pz = (ZoomRateDef)ZoomStru;
	f1 = pz.PredictCurve1;
	f2 = pz.PredictCurve2;
	if( pz.Extra)
	{
		next_focus_pos = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, (SW_INT16*)f1);
	}
	else
	{
		focus_next_max = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, (SW_INT16*)f1);
		focus_next_min = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, (SW_INT16*)f2);
		next_focus_pos = focus_next_min + (SW_INT)(pz.Rate * SW_ABS(focus_next_max - focus_next_min) + 0.5);
	}
	if(IPZ_ZBC_LENS_GetZoomDir()&&FirstWideFlag)
	{
		ZoomTMaxFocus = IPZ_ZBC_LENS_GetFocusCurPos();
		ZoomTMaxZoom = IPZ_ZBC_LENS_GetZoomCurPos();
		focus_next_max = IPZ_ZBC_CURVE_GetCurveFocus(ZoomTMaxZoom, upper_curve);
		focus_next_min = IPZ_ZBC_CURVE_GetCurveFocus(ZoomTMaxZoom, lower_curve);
		ZoomTRatio = (SW_FLOAT)(ZoomTMaxFocus - focus_next_min)/(SW_FLOAT)(focus_next_max - focus_next_min);
		FirstWideFlag = 0;
	}
	else
	{
		FirstWideFlag = 1;
		if(0 == IfADD)
		{
			next_focus_pos +=  0;
		}
		else if(1 == IfADD)
		{
			next_focus_pos += AdjStep;
		}
		else if(-1 == IfADD)
		{
			next_focus_pos -= AdjStep;
		}
		else
		{
			next_focus_pos -= AdjStep;
		}
		focus_next_max = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, upper_curve);
		focus_next_min = IPZ_ZBC_CURVE_GetCurveFocus(zoom_p, lower_curve);
		next_focus_pos = next_focus_pos > focus_next_max? focus_next_max : next_focus_pos;
		next_focus_pos = next_focus_pos < focus_next_min? focus_next_min : next_focus_pos;
	}
	stepDiff = SW_ABS(next_focus_pos - focus);
	if( focus < next_focus_pos)
	{
		IPZ_ZBC_LENS_SetFocusDir(SW_B_FALSE);
		if(next_focus_pos > IPZ_ZBC_LENS_GetLensFocusMinMax(SW_B_TRUE))
		{
			next_focus_pos = IPZ_ZBC_LENS_GetLensFocusMinMax(SW_B_TRUE);
			stepDiff = SW_ABS(next_focus_pos - focus);
		}
	}
	else if( focus >= next_focus_pos)
	{
		IPZ_ZBC_LENS_SetFocusDir(SW_B_TRUE);
		if(next_focus_pos < IPZ_ZBC_LENS_GetLensFocusMinMax(SW_B_FALSE))
		{
			next_focus_pos = IPZ_ZBC_LENS_GetLensFocusMinMax(SW_B_FALSE);
			stepDiff = SW_ABS(next_focus_pos - focus);
		}
	}
	IPZ_ZBC_LENS_SetFocusStepNum(stepDiff);
	if( stepDiff > IPZ_ZBC_LENS_GetFocusStepNum())
	{
		IPZ_ZBC_LENS_SetFocusStepNum(IPZ_ZBC_LENS_GetFocusStepNum());
	}
	return SW_B_TRUE;
}
void GeneratePredictCurve(void)
{
    SW_INT i;
    SW_INT focus, zoom;
    SW_INT range_to_upper, range_to_lower, bell_length;
    SW_FLOAT rate;
    SW_ThreadMSleep(80);
    zoom = IPZ_ZBC_LENS_GetZoomCurPos();
    focus = IPZ_ZBC_LENS_GetFocusCurPos();
    range_to_upper = SW_ABS(focus - IPZ_ZBC_CURVE_GetCurveFocus(zoom, Lens1M5));
    range_to_lower = SW_ABS(focus - IPZ_ZBC_CURVE_GetCurveFocus(zoom, LensInf));
    bell_length =  range_to_upper + range_to_lower;
    rate = (SW_FLOAT)(range_to_lower) / (SW_FLOAT)(bell_length);
    for(i = 0; i < IPZ_ZBC_LENS_GetCurveNum(); i++)
    {
        LensPredictCurve[i] =(SW_INT) (rate*SW_ABS(Lens1M5[i] - LensInf[i]) + LensInf[i]);
    }
}
SW_UINT GetZoomTVersion(void)
{
    SW_UINT v;
    SW_INT  year, month, day;
    SW_UINT8 plat,v1,v2;
    SW_INT8 m[4];
    SW_CHAR *months[] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"};
    plat = 1;
    v1 = ZOOMT_VERSION / 100;
    v2 = ZOOMT_VERSION % 100;
    sscanf(__DATE__, "%s %d %d", m, &day, &year);
    for(month=0; month<12; month++)
    {
        if(SW_MemCmp(m, months[month],1) == 0)
        {
            break;
        }
    }
    month++;
    year -= 2000;
    v = (plat << 27) + (v1 << 23) + (v2 << 16) + (year << 9) + (month << 5) + (day << 0);
    return v;
}
void PrintZoomTVersion(void)
{
    SW_INT v;
    SW_UINT plat,v1,v2,year,month,day;
    v = GetZoomTVersion();
    plat = (v & 0xf8000000) >> 27;
    v1 = (v & 0x07800000) >> 23;
    v2 = (v & 0x007f0000) >> 16;
    year = (v & 0x0000fe00) >> 9;
    month = (v & 0x000001e0) >> 5;
    day = (v & 0x0000001f) >> 0;
    return;
}
